//
// Created by daybeha on 2022/7/13.
//

#include <ros/ros.h>
#include "superglue/SuperScore.h"
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>

using namespace std;

// 图像拼接
cv::Mat ImageSplicing(vector<cv::Mat> images,int type)
{
    if (type != 0 && type != 1)
        type = 0;

    int num = images.size();
    int newrow = 0;
    int newcol = 0;
    cv::Mat result;

    // 横向拼接
    if (type == 0)
    {
        int minrow = 10000;
        for (int i = 0; i < num; ++i)
        {
            if (minrow > images[i].rows)
                minrow = images[i].rows;
        }
        newrow = minrow;
        for (int i = 0; i < num; ++i)
        {
            int tcol = images[i].cols*minrow / images[i].rows;
            int trow = newrow;
            cv::resize(images[i], images[i], cv::Size(tcol, trow));
            newcol += images[i].cols;
            if (images[i].type() != images[0].type())
                images[i].convertTo(images[i], images[0].type());
        }
        result = cv::Mat(newrow, newcol, images[0].type(), cv::Scalar(255, 255, 255));

        cv::Range rangerow, rangecol;
        int start = 0;
        for (int i = 0; i < num; ++i)
        {
            rangerow = cv::Range((newrow - images[i].rows) / 2, (newrow - images[i].rows) / 2 + images[i].rows);
            rangecol = cv::Range(start, start + images[i].cols);
            images[i].copyTo(result(rangerow, rangecol));
            start += images[i].cols;
        }
    }
        // 纵向拼接
    else if (type == 1) {
        int mincol = 10000;
        for (int i = 0; i < num; ++i)
        {
            if (mincol > images[i].cols)
                mincol = images[i].cols;
        }
        newcol = mincol;
        for (int i = 0; i < num; ++i)
        {
            int trow = images[i].rows*mincol / images[i].cols;
            int tcol = newcol;
            cv::resize(images[i], images[i], cv::Size(tcol, trow));
            newrow += images[i].rows;
            if (images[i].type() != images[0].type())
                images[i].convertTo(images[i], images[0].type());
        }
        result = cv::Mat(newrow, newcol, images[0].type(), cv::Scalar(255, 255, 255));

        cv::Range rangerow, rangecol;
        int start = 0;
        for (int i = 0; i < num; ++i)
        {
            rangecol= cv::Range((newcol - images[i].cols) / 2, (newcol - images[i].cols) / 2 + images[i].cols);
            rangerow = cv::Range(start, start + images[i].rows);
            images[i].copyTo(result(rangerow, rangecol));
            start += images[i].rows;
        }
    }

    return result;
}


// service回调函数，输入参数req，输出参数res
bool cam_result_callback(superglue::SuperScore::Request  &req,
                         superglue::SuperScore::Response &res)
{
    ROS_INFO("received request");

    cv_bridge::CvImagePtr cv_ptr0,cv_ptr1;
    //图像有消息格式转换成Mat格式
//    cv_ptr0 = cv_bridge::toCvCopy(req.image0, sensor_msgs::image_encodings::BGR8);
//    cv_ptr1 = cv_bridge::toCvCopy(req.image1, sensor_msgs::image_encodings::BGR8);
    cv_ptr0 = cv_bridge::toCvCopy(req.image0, sensor_msgs::image_encodings::MONO8);
    cv_ptr1 = cv_bridge::toCvCopy(req.image1, sensor_msgs::image_encodings::MONO8);

    vector<cv::Mat> images = {cv_ptr0->image, cv_ptr1->image};
    cv::Mat ret_img = ImageSplicing(images, 1);
    cv::imshow("image in server", ret_img);
    cv::waitKey(1);

    return true;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "server_img");
    ros::NodeHandle node;

    // 创建一个名为/cam的server，注册回调函数cam_result_callback
    ros::ServiceServer img_service = node.advertiseService("/super_score", cam_result_callback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

